#include "cartographer/rcm/dynamic/dynamic_detect.h"

namespace cartographer
{
    mapping::MapLimits GrowMapLimits(mapping::MapLimits &limitOrg, const Eigen::Vector2f &point)
    {
        // 栅格地图原点在右下角，max()返回的是左上角的物理坐标
        while (!limitOrg.Contains(limitOrg.GetCellIndex(point)))
        {
            // 栅格地图的中心索引
            const int num_x = limitOrg.cell_limits().num_x_cells;
            const int num_y = limitOrg.cell_limits().num_y_cells;
            const int x_offset = num_x / 2;
            const int y_offset = num_y / 2;
            auto max_new = limitOrg.max(); // max返回的是const，不要直接用max()做加法，否则limit的max值不会改变
            max_new = max_new + limitOrg.resolution() * Eigen::Vector2d(y_offset, x_offset);
            auto cell_limit_new = mapping::CellLimits(2 * num_x, 2 * num_y);
            // 由栅格地图中心向外扩大一倍
            const mapping::MapLimits new_limits(limitOrg.resolution(), max_new, cell_limit_new);
            limitOrg = new_limits;
        }
        return limitOrg;
    }

    mapping::MapLimits GrowMapLimits(const sensor::RangeData &range_data, mapping::ProbabilityGrid *const probability_grid)
    {
        Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
        // Padding around bounding box to avoid numerical issues at cell boundaries.
        constexpr float kPadding = 1e-6f;
        for (const sensor::RangefinderPoint &hit : range_data.returns)
        {
            bounding_box.extend(hit.position.head<2>());
        }
        for (const sensor::RangefinderPoint &miss : range_data.misses)
        {
            bounding_box.extend(miss.position.head<2>());
        }
        mapping::MapLimits limits = probability_grid->limits();
        limits = GrowMapLimits(limits, bounding_box.min() - kPadding * Eigen::Vector2f::Ones());
        limits = GrowMapLimits(limits, bounding_box.max() + kPadding * Eigen::Vector2f::Ones());
        return limits;
    }

    mapping::MapLimits ScaleGridResolution(const mapping::MapLimits &limits, const float pixelScale)
    {
        const double resolutionOrg = limits.resolution();
        const double resolutionNew = resolutionOrg / pixelScale;
        int maxIdxX = limits.cell_limits().num_x_cells;
        int maxIdxY = limits.cell_limits().num_y_cells;
        mapping::CellLimits cellLimits(maxIdxX * pixelScale, maxIdxY * pixelScale);
        mapping::MapLimits scaledLimits(resolutionNew, limits.max(), cellLimits);
        LOG(INFO) << "[dyn] ScaleResolution scale " << pixelScale;
        LOG(INFO) << "[dyn] resolution org " << resolutionOrg << " cellIdx x " << maxIdxX << " y " << maxIdxY;
        LOG(INFO) << "[dyn] resolution new " << resolutionNew << " cellIdx x " << cellLimits.num_x_cells << " y " << cellLimits.num_y_cells;
        return scaledLimits;
    }

    SingleDynamicDetector::SingleDynamicDetector() : DynamicDetector()
    {
    }

    void SingleDynamicDetector::DetectDynPoint(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time,
                                               std::vector<int> &cnts)
    {
        mapping::ProbabilityGrid *const pgrid = static_cast<mapping::ProbabilityGrid *>(grid);
        CHECK(pgrid != nullptr);
        auto &option = DynamicOptions::GetInstance();
        // scale map limits
        const auto mapLimits = GrowMapLimits(range_data, pgrid);
        const auto scaledLimits = ScaleGridResolution(mapLimits, option.subpixelScale);
        // push hit
        int hitSize = (int)range_data.returns.size();
        PrepareCloud(time, hitSize);
        std::map<int, int> pickedTimes;
        LOG(INFO) << "[dyn] time " << time << " cacheSize " << cloudQueue_.size() << " hitSize " << hitSize;
        for (const auto &hit : range_data.returns)
        {
            auto cellIdx = scaledLimits.GetCellIndex(hit.position.head<2>());
            int index = GetLinearIndex(cellIdx, scaledLimits);
            // new point
            auto point = std::make_shared<DynPoint>();
            point->obsolete = false;
            point->hit = hit;
            point->index = index;
            point->cellIdx = cellIdx;
            point->orgIndex = cloudQueue_.back()->Size();
            cloudQueue_.back()->points_.push_back(point);
            // check same index
            if (pickedTimes.find(index) == pickedTimes.end())
            {
                if (hitCntMap_.find(index) == hitCntMap_.end())
                    hitCntMap_[index] = 1;
                else
                    hitCntMap_[index]++;
                pickedTimes[index] = 1;
            }
            else
                pickedTimes[index]++;
            // LOG(INFO) << "[dyn] cell " << cellIdx(0) << " " << cellIdx(1) << " cnt " << hitCntMap_[index];
        }
        int duplicateCnt = 0;
        int duplicateCntMax = -1;
        for (auto &iter : pickedTimes)
        {
            if (duplicateCntMax < 0)
                duplicateCntMax = iter.second;
            else if (iter.second > duplicateCntMax)
                duplicateCntMax = iter.second;
            if (iter.second > 1)
                duplicateCnt++;
        }
        int queueSize = (int)cloudQueue_.size();
        if (queueSize > 1)
        {
            double duration = cloudQueue_[queueSize - 1]->time_ - cloudQueue_[queueSize - 2]->time_;
            LOG(INFO) << "[dyn] queueSize " << queueSize << " duration " << duration;
        }
        LOG(INFO) << "[dyn] hitSize " << hitSize << " duplicateSize " << duplicateCnt << " maxCnt " << duplicateCntMax;
        if (queueSize < option.cacheSize)
            return;
        int motionCnt = 0;
        cnts.reserve(hitSize);
        CHECK(cloudQueue_.back()->Size() == hitSize);
        for (const auto &point : cloudQueue_.back()->points_)
        {
            int hitCnt = hitCntMap_[point->index];
            cnts.push_back(hitCnt);
            if (hitCnt < option.motionSize)
                motionCnt++;
        }
        for (size_t i = 0; i < cnts.size(); i++)
        {
            LOG(INFO) << "[dyn] " << i << " " << cnts[i];
        }
        LOG(INFO) << "[dyn] hitSize " << hitSize << " motionCnt " << motionCnt;
    }

    void SingleDynamicDetector::PrepareCloud(const common::Time time, int size)
    {
        while ((int)cloudQueue_.size() >= DynamicOptions::GetInstance().cacheSize)
        {
            std::vector<int> picked;
            auto &cloud = cloudQueue_.front();
            for (auto &hit : cloud->points_)
            {
                // important
                if (std::find(picked.begin(), picked.end(), hit->index) == picked.end())
                {
                    picked.push_back(hit->index);
                    hit->obsolete = true;
                    hitCntMap_[hit->index]--;
                }
            }
            cloudQueue_.pop_front();
            LOG(INFO) << "[dyn] popCloud " << cloud->Size() << " time " << cloud->time_;
        }
        auto cloud = std::make_shared<DynCloud>();
        cloud->points_.reserve(size);
        cloud->time_ = common::ToUniversal(time);
        cloudQueue_.push_back(cloud);
    }
} // namespace cartographer